#ifndef CLIENT_H
#define CLIENT_H

#include "../include/px4control.h"
#include "../../../devel/include/Px4service/controlcmd.h"

class px4client : public px4control{
public:
    px4client(ros::NodeHandle *n):px4control(n){
        task_running = false;
        client = n->serviceClient<Px4service::controlcmd>("/Px4service/controlcmd");
    }

    void create_task(geometry_msgs::PoseStamped Pose){
        Px4service::controlcmd srv;

        srv.request.modify = true;
        srv.request.p_x = Pose.pose.position.x;
        srv.request.p_y = Pose.pose.position.y;
        srv.request.p_z = Pose.pose.position.z;

        if (client.call(srv)) {
            ROS_INFO("-----当前服务器参数-----");
            ROS_INFO("X:%f,Y:%f,Z:%f RUN: %s", srv.response.p_x,srv.response.p_y,srv.response.p_z, srv.response.run?"true":"false");
            ROS_INFO("----------------------");
            task_running = srv.response.run;
            service_task.pose.position.x = srv.response.p_x;
            service_task.pose.position.y = srv.response.p_y;
            service_task.pose.position.z = srv.response.p_z;
        } else {
            ROS_ERROR("Failed to call service string_service");
        }
    }

    void operate_task(){
        Px4service::controlcmd srv;
        srv.request.modify = false;
        srv.request.run = true;
        if (client.call(srv)) {
            ROS_INFO("-----当前服务器参数-----");
            ROS_INFO("X:%f,Y:%f,Z:%f RUN: %s", srv.response.p_x,srv.response.p_y,srv.response.p_z, srv.response.run?"true":"false");
            ROS_INFO("----------------------");
            task_running = srv.response.run;
        } else {
            ROS_ERROR("Failed to call service string_service");
        }
    }
geometry_msgs::PoseStamped service_task;
bool task_running;
ros::ServiceClient client;
};

#endif